/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    ADXL345_I2C accelerometer = new ADXL345_I2C(4, ADXL345_I2C.DataFormat_Range.k2G);

    int previousValue = 0; // the binary value from the previous loop//ttes
    boolean goLeft = false;
    RobotDrive drive = new RobotDrive(1, 2);
    //Joystick gamepad = new Joystick(1);
    Joystick leftStick = new Joystick(1);
    Joystick rightStick = new Joystick(2);
    Compressor compressor = new Compressor(3, 3);
    Solenoid kick = new Solenoid(7, 1);
    Solenoid retract = new Solenoid(7, 2);
    boolean kicking = false;
    long kickerEngaged;
    Watchdog watchdog = getWatchdog();
    DigitalInput left; // digital inputs for line tracking sensors
    DigitalInput middle;
    DigitalInput right;
    DriverStation ds; // driver station object for getting selections
    double defaultSteeringGain = -0.85; // the default value for the steering gain
    Gyro gyro = new Gyro(2);
    double previousTime;
    double time;
    int stopTime = 10;
    boolean atCross = false;
    Timer timer = new Timer();
    double gyroTurn = .65;

    double xPosition = 0;
    double xVelocity = 0;
    double yPosition = 0;
    double yVelocity = 0;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        compressor.start();
        //create digital input objects to read from sensors
        left = new DigitalInput(14);
        middle = new DigitalInput(10);
        right = new DigitalInput(5);

        //get driver station instance to read the digital I/O pins
        ds = DriverStation.getInstance();
    }

    public void autonomousInit() {
        timer.start();
        timer.reset();
        gyro.reset();
        getWatchdog().kill();
        previousTime = timer.get();
        //int binaryValue; // a single binary value of the three line tracking
        // sensors
        //int previousValue = 0; // the binary value from the previous loop
        //double steeringGain; // the amount of steering correction to apply

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        ADXL345_I2C.AllAxes accelerations = accelerometer.getAccelerations();
        time = timer.get();
        double timeChange = time - previousTime;
        double xVelocityOld = xVelocity;
        double yVelocityOld = yVelocity;
        xVelocity += timeChange*accelerations.XAxis;
        yVelocity += timeChange*accelerations.YAxis;
        xPosition += (xVelocity + xVelocityOld) / 2 * timeChange;
        yPosition += (yVelocity + yVelocityOld) / 2 * timeChange;
        previousTime = time;
        System.out.println("x acceleration" + accelerations.XAxis);
         System.out.println("y acceleration" + accelerations.YAxis);
        System.out.println("X Velocity: " + xVelocity + "\nY Velocity: " + yVelocity + "\nX Position: " + xPosition + "\nY Position: " + yPosition);
   /*     if (!atCross) {
            int binaryValue; // a single binary value of the three line tracking
            // sensors

            double steeringGain;
            double speed, turn;

            int leftValue = left.get() ? 1 : 0;
            int middleValue = middle.get() ? 1 : 0;
            int rightValue = right.get() ? 1 : 0;
            // compute the single value from the 3 sensors. Notice that the bits
            // for the outside sensors are flipped depending on left or right
            // fork. Also the sign of the steering direction is different for left/right.
            if (goLeft) {
                binaryValue = leftValue * 4 + middleValue * 2 + rightValue;
                steeringGain = -defaultSteeringGain;
            } else {
                binaryValue = rightValue * 4 + middleValue * 2 + leftValue;
                steeringGain = defaultSteeringGain;
            }

            // get the default speed and turn rate at this time
            speed = .5;
            turn = 0;

            // different cases for different line tracking sensor readings
            switch (binaryValue) {
                case 1:  // on line edge
                    turn = 0;
                    break;
                case 7:  // all sensors on (maybe at cross)
                    if (time > stopTime) {
                        atCross = true;
                        speed = 0;
                    }
                    break;
                case 0:  // all sensors off
                    if (previousValue == 0 || previousValue == 1) {
                        turn = steeringGain;
                    } else {
                        turn = -steeringGain;
                    }
                    break;
                default:  // all other cases
                    turn = -steeringGain;
            }
            // print current status for debugging
            if (binaryValue != previousValue) {
                System.out.println("Time: " + time + " Sensor: " + binaryValue + " speed: " + speed + " turn: " + turn + " atCross: " + atCross);
            }

            // set the robot speed and direction
            drive.arcadeDrive(speed, turn);

            if (binaryValue != 0) {
                previousValue = binaryValue;
            }
        }
        else if (Math.abs(gyro.getAngle()) > .5) {
            if(gyro.getAngle() < 0) drive.tankDrive(-gyroTurn, gyroTurn);
            if(gyro.getAngle() > 0) drive.tankDrive(gyroTurn, -gyroTurn);
        }
 //       Timer.delay(0.01);

        //watchdog.feed();
*/
        if(-xPosition / 9.81 < 1.5) {
            drive.tankDrive(.75, .75);
        }

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        watchdog.feed();
        //drive.tankDrive(gamepad.getRawAxis(4)*-1, gamepad.getRawAxis(2)*-1);
        drive.tankDrive(leftStick.getY() * -1, rightStick.getY() * -1);
        if (this.isOperatorControl() && this.isEnabled() && leftStick.getRawButton(11)) {
            compressor.start();
        }
        if (leftStick.getRawButton(10)) {
            compressor.stop();
        }

        //engages kicker
        if (leftStick.getRawButton(1) && !kicking) {
            System.out.println("trying to kick");
            kicking = true;
            kick.set(true);
            retract.set(false);
            kickerEngaged = System.currentTimeMillis();
        }

        //retracts kicker after 500 ms
        if (kicking) {
            if (System.currentTimeMillis() > kickerEngaged + 500) {
                System.out.println("trying to retract");
                kicking = false;
                kick.set(false);
                retract.set(true);
            }
        }

    }
}
